#include "PID.hpp"
#include "utils.hpp"
#include <iostream>

PID::PID(float P, float I, float D)
{
    SetP(P);
    SetI(I);
    SetD(D);
    Reset();
}

void PID::SetP(float value)
{
    P = value;
}

void PID::SetI(float value)
{
    I = value;
}

void PID::SetD(float value)
{
    D = value;
}

float PID::GetControl(float error, float timeDelta)
{
    errorSum += error * timeDelta;
    float derror = (error - lastError) / timeDelta;
    float ctrl = P * error + I * errorSum + D * derror;
    lastError = error;
    return ctrl;
}
float PID::GetControlWithClamp(float error, float timeDelta, float range)
{
    errorSum += error * timeDelta;
    float derror = (error - lastError) / timeDelta;
    float sum = P * error + I * errorSum + D * derror;
    lastError = error;
    float ctrl = ABSClamp(sum, range);
    bool isClamp = ctrl != sum;
    bool sameSign = HasSameSign(error, errorSum);
    if (isClamp && sameSign)
    {
        errorSum -= error;
    }
    return ctrl;
}
float PID::GetControlWithDError(float error, float deltaError, float timeDelta)
{
    errorSum += error * timeDelta;
    float ctrl = P * error + I * errorSum + D * deltaError;
    lastError = error;
    // std::cout << errorSum << ":" << error << ":" << timeDelta << std::endl;
    return ctrl;
}

void PID::Reset()
{
    errorSum = 0;
    lastError = 0;
}
